#!/usr/bin/env python
# Copyright 2020 Makani Technologies LLC
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#      http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

# -*- coding: utf-8 -*-
# generated by wxGlade 0.6.3 on Fri Jun 15 11:57:59 2012
import ctypes
import os
import socket
import sys

import gflags
from makani.avionics.common import aio
from makani.avionics.common import pack_avionics_messages
from makani.lib.python import c_helpers
from makani.sim import pack_sim_messages
from makani.system import labels
import wx

FLAGS = gflags.FLAGS


class JoystickFrame(wx.Frame):
    def __init__(self, *args, **kwds):
        # begin wxGlade: JoystickFrame.__init__
        kwds["style"] = wx.DEFAULT_FRAME_STYLE
        wx.Frame.__init__(self, *args, **kwds)
        self.release_toggle_button = wx.ToggleButton(self, -1, "Release")
        self.save_state_button = wx.Button(self, -1, "Save State")
        self.pilot_switch_radio_box = wx.RadioBox(
            self, -1, "Pilot switch",
            choices=["Pilot hover", "Semi-autonomous", "Autonomous"], majorDimension=3,
            style=wx.RA_SPECIFY_COLS)
        self.static_line = wx.StaticLine(self, -1)
        self.throt_slider = wx.Slider(
            self, -1, 0, 0, 100,
            style=wx.SL_VERTICAL|wx.SL_AUTOTICKS|wx.SL_LEFT|wx.SL_INVERSE)
        self.rud_slider = wx.Slider(
            self, -1, 0, -100, 100,
            style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_BOTTOM)
        self.ail_slider = wx.Slider(
            self, -1, 0, -100, 100,
            style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_BOTTOM)
        self.ele_slider = wx.Slider(
            self, -1, 0, -100, 100,
            style=wx.SL_VERTICAL|wx.SL_AUTOTICKS|wx.SL_LEFT|wx.SL_INVERSE)

        text_style = (wx.TE_PROCESS_ENTER | wx.TE_PROCESS_TAB | wx.TE_CENTRE
                      | wx.NO_BORDER)
        self.throt_text_ctrl = wx.TextCtrl(self, -1, "0", style=text_style)
        self.rud_text_ctrl = wx.TextCtrl(self, -1, "0", style=text_style)
        self.ail_text_ctrl = wx.TextCtrl(self, -1, "0", style=text_style)
        self.ele_text_ctrl = wx.TextCtrl(self, -1, "0", style=text_style)

        self.__set_properties()
        self.__do_layout()

        self.Bind(wx.EVT_BUTTON, self.on_save_state, self.save_state_button)
        self.Bind(wx.EVT_COMMAND_SCROLL, self.on_throt_change,
                  self.throt_slider)
        self.Bind(wx.EVT_COMMAND_SCROLL, self.on_rud_change, self.rud_slider)
        self.Bind(wx.EVT_COMMAND_SCROLL, self.on_ail_change, self.ail_slider)
        self.Bind(wx.EVT_COMMAND_SCROLL, self.on_ele_change, self.ele_slider)
        self.Bind(wx.EVT_TEXT_ENTER, self.on_throt_text_change,
                  self.throt_text_ctrl)
        self.Bind(wx.EVT_TEXT_ENTER, self.on_rud_text_change,
                  self.rud_text_ctrl)
        self.Bind(wx.EVT_TEXT_ENTER, self.on_ail_text_change,
                  self.ail_text_ctrl)
        self.Bind(wx.EVT_TEXT_ENTER, self.on_ele_text_change,
                  self.ele_text_ctrl)
        # end wxGlade

        self.save_state_pressed = 0

    def __set_properties(self):
        # begin wxGlade: JoystickFrame.__set_properties
        self.SetTitle("Joystick")
        self.SetSize((480, 360))
        self.pilot_switch_radio_box.SetSelection(0)
        # end wxGlade

    def __do_layout(self):
        # begin wxGlade: JoystickFrame.__do_layout
        main_sizer = wx.BoxSizer(wx.VERTICAL)
        label_sizer = wx.BoxSizer(wx.HORIZONTAL)
        stick_sizer = wx.BoxSizer(wx.HORIZONTAL)
        switch_sizer = wx.BoxSizer(wx.HORIZONTAL)
        switch_sizer.Add(self.release_toggle_button, 1,
                         wx.ALIGN_CENTER_VERTICAL, 0)
        switch_sizer.Add(self.pilot_switch_radio_box, 3,
                         wx.EXPAND|wx.ALIGN_CENTER_VERTICAL, 0)
        switch_sizer.Add(self.save_state_button, 1, wx.ALIGN_CENTER_VERTICAL, 0)
        main_sizer.Add(switch_sizer, 3, wx.EXPAND, 0)
        main_sizer.Add(self.static_line, 1, wx.EXPAND, 0)
        stick_sizer.Add(self.throt_slider, 1, wx.EXPAND, 0)
        stick_sizer.Add(self.rud_slider, 1, wx.EXPAND, 1)
        stick_sizer.Add(self.ail_slider, 1, wx.EXPAND|wx.ALIGN_CENTER_VERTICAL,
                        1)
        stick_sizer.Add(self.ele_slider, 1, wx.EXPAND, 0)
        main_sizer.Add(stick_sizer, 7, wx.EXPAND, 1)
        label_sizer.Add(self.throt_text_ctrl, 1, wx.ALIGN_CENTER_HORIZONTAL, 0)
        label_sizer.Add(self.rud_text_ctrl, 1, wx.ALIGN_CENTER_HORIZONTAL, 0)
        label_sizer.Add(self.ail_text_ctrl, 1, wx.ALIGN_CENTER_HORIZONTAL, 0)
        label_sizer.Add(self.ele_text_ctrl, 1, wx.ALIGN_CENTER_HORIZONTAL, 0)
        main_sizer.Add(label_sizer, 1, wx.EXPAND, 0)
        self.SetSizer(main_sizer)
        self.Layout()
        # end wxGlade

    def on_save_state(self, event):
        self.save_state_pressed = 1

    def poll_save_state(self):
        pressed = self.save_state_pressed
        self.save_state_pressed = 0
        return pressed

    def on_throt_text_change(self, event): # wxGlade: JoystickFrame.<event_handler>
        try:
            throt = float(self.throt_text_ctrl.GetValue())
            self.throt_slider.SetValue(int(throt*100))
        except:
            pass
        self.throt_text_ctrl.SetValue(str(self.throt_slider.GetValue()/100.0))

    def on_rud_text_change(self, event): # wxGlade: JoystickFrame.<event_handler>
        try:
            rud = float(self.rud_text_ctrl.GetValue())
            self.rud_slider.SetValue(int(rud*100))
        except:
            pass
        self.rud_text_ctrl.SetValue(str(self.rud_slider.GetValue()/100.0))

    def on_ail_text_change(self, event): # wxGlade: JoystickFrame.<event_handler>
        try:
            ail = float(self.ail_text_ctrl.GetValue())
            self.ail_slider.SetValue(int(ail*100))
        except:
            pass
        self.ail_text_ctrl.SetValue(str(self.ail_slider.GetValue()/100.0))

    def on_ele_text_change(self, event): # wxGlade: JoystickFrame.<event_handler>
        try:
            ele = float(self.ele_text_ctrl.GetValue())
            self.ele_slider.SetValue(int(ele*100))
        except:
            pass
        self.ele_text_ctrl.SetValue(str(self.ele_slider.GetValue()/100.0))

    def on_throt_change(self, event): # wxGlade: JoystickFrame.<event_handler>
        self.throt_text_ctrl.SetValue(str(self.GetThrottle()))

    def on_rud_change(self, event): # wxGlade: JoystickFrame.<event_handler>
        self.rud_text_ctrl.SetValue(str(self.GetYaw()))

    def on_ail_change(self, event): # wxGlade: JoystickFrame.<event_handler>
        self.ail_text_ctrl.SetValue(str(self.GetRoll()))

    def on_ele_change(self, event): # wxGlade: JoystickFrame.<event_handler>
        self.ele_text_ctrl.SetValue(str(self.GetPitch()))

    def GetThrottle(self):
        return self.throt_slider.GetValue() / 100.0

    def GetRoll(self):
        return self.ail_slider.GetValue() / 100.0

    # The pitch sitck is inverted - pulling it down produces a positive output.
    def GetPitch(self):
        return -float(self.ele_slider.GetValue()) / 100.0

    def GetYaw(self):
        return self.rud_slider.GetValue() / 100.0

# end of class JoystickFrame


class Joystick(wx.App):
    def __init__(self, *args, **kwargs):
        wx.App.__init__(self, *args, **kwargs)

        self._seq = 0
        self._aio_client = aio.AioClient(['kMessageTypeSimCommand',
                                          'kMessageTypeJoystickStatus'])

    def OnInit(self):
        wx.InitAllImageHandlers()
        self.joystick_frame = JoystickFrame(None, -1, "")
        self.SetTopWindow(self.joystick_frame)
        self.joystick_frame.Show()

        self.joystick_frame.throt_text_ctrl.SetValue('0.0')
        self.joystick_frame.rud_text_ctrl.SetValue('0.0')
        self.joystick_frame.ail_text_ctrl.SetValue('0.0')
        self.joystick_frame.ele_text_ctrl.SetValue('0.0')
        self.joystick_frame.pilot_switch_radio_box.SetSelection(0)

        self.publish_timer = wx.Timer(self)
        self.Bind(wx.EVT_TIMER, self.on_publish, self.publish_timer)
        self.publish_timer.Start(100)

        return 1

    def on_publish(self, event):
        frame = self.joystick_frame

        throt = frame.GetThrottle()
        ail = frame.GetRoll()
        ele = frame.GetPitch()
        rud = frame.GetYaw()

        if frame.pilot_switch_radio_box.GetSelection() == 0:
            pilot_switch = labels.kJoystickSwitchPositionUp
        elif frame.pilot_switch_radio_box.GetSelection() == 1:
            pilot_switch = labels.kJoystickSwitchPositionMiddle
        else:
            pilot_switch = labels.kJoystickSwitchPositionDown

        if frame.release_toggle_button.GetValue():
            release = labels.kJoystickSwitchPositionUp
        else:
            release = labels.kJoystickSwitchPositionDown

        command = pack_sim_messages.kSimRecordStateCommandDont
        if frame.poll_save_state():
            command = pack_sim_messages.kSimRecordStateCommandOverwrite
        sim_command = pack_sim_messages.SimCommandMessage()
        sim_command.record_mode = command
        self._aio_client.Send(sim_command, 'kMessageTypeSimCommand',
                              'kAioNodeSimulatedJoystick')

        joystick_message = pack_avionics_messages.JoystickStatusMessage()
        joystick_message.roll = ail
        joystick_message.pitch = ele
        joystick_message.yaw = rud
        joystick_message.throttle = throt
        joystick_message.tri_switch = pilot_switch
        joystick_message.momentary_switch = release

        self._aio_client.Send(joystick_message, 'kMessageTypeJoystickStatus',
                              'kAioNodeSimulatedJoystick')
        self._seq += 1

# end of class Joystick

def main(argv):
    # Parse flags.
    try:
        argv = FLAGS(argv)
    except gflags.FlagsError, e:
        print '\nError: %s\n\nUsage: %s ARGS\n%s' % (e, sys.argv[0], FLAGS)
        sys.exit(1)

    Joystick().MainLoop()


if __name__ == '__main__':
    main(sys.argv)
